#include "control_node.h"

#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>

#include <uORB/topics/parameter_update.h>


int ControlNode::print_status()
{
    PX4_INFO("Running");
    // TODO: print additional runtime information about the state of the module

    return 0;
}

int ControlNode::custom_command(int argc, char *argv[])
{
    /*
    if (!is_running()) {
        print_usage("not running");
        return 1;
    }

    // additional custom commands can be handled like this:
    if (!strcmp(argv[0], "do-something")) {
        get_instance()->do_something();
        return 0;
    }
     */

    return print_usage("unknown command");
}


int ControlNode::task_spawn(int argc, char *argv[])
{
    _task_id = px4_task_spawn_cmd("module",
                                  SCHED_DEFAULT,
                                  SCHED_PRIORITY_DEFAULT,
                                  1024,
                                  (px4_main_t)&run_trampoline,
                                  (char *const *)argv);

    if (_task_id < 0)
    {
        _task_id = -1;
        return -errno;
    }

    return 0;
}

ControlNode *ControlNode::instantiate(int argc, char *argv[])
{
    int example_param = 0;
    bool example_flag = false;
    bool error_flag = false;

    int myoptind = 1;
    int ch;
    const char *myoptarg = nullptr;

    // parse CLI arguments
    while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF)
    {
        switch (ch)
        {
            case 'p':
                example_param = (int)strtol(myoptarg, nullptr, 10);
                break;

            case 'f':
                example_flag = true;
                break;

            case '?':
                error_flag = true;
                break;

            default:
                PX4_WARN("unrecognized flag");
                error_flag = true;
                break;
        }
    }

    if (error_flag)
    {
        return nullptr;
    }

    ControlNode *instance = new ControlNode(example_param, example_flag);

    if (instance == nullptr)
    {
        PX4_ERR("alloc failed");
    }

    return instance;
}

ControlNode::ControlNode(int example_param, bool example_flag)
    : ModuleParams(nullptr)
{
}

void ControlNode::run()
{
    //记录起始点位置
    float begin_x;
    float begin_y;
    float begin_z;
    float target_x;
    float target_y;
    float begin_yaw;
    float gain=0;
   // float gainbuf[10]={0.001,0.003,0.007,0.018,0.038,0.08,0.18,0.38,0.7,1};
    float vel_max,cur_vel_max=0;

    float_t xy_rad=1;
    float_t z_rad=1;
    //float_t yaw_rad=0.1;
//进入offboard模式并解锁


    while(!should_exit())
    {

//                 _vehicle_local_position_sub.copy(&_vehicle_local_position);
//         _follow_target_sub.copy(&_target);
//                 _global_local_proj_ref.initReference(_vehicle_local_position.ref_lat,_vehicle_local_position.ref_lon,hrt_absolute_time());
//         _global_local_proj_ref.project(_target.lat,_target.lon,target_x,target_y);
//                _a02_dist_to_target.dist_to_target=(double)sqrtf((_vehicle_local_position.x-target_x)*(_vehicle_local_position.x-target_x)+(_vehicle_local_position.y-target_y)*(_vehicle_local_position.y-target_y));
// _a02_dist_to_target_pub.publish(_a02_dist_to_target);

        _test_mavlink_rx_sub.copy(&_test_mavlink_rx);
//         PX4_INFO("test1=%d\n",_test_mavlink_rx.test1);
//  PX4_INFO("test2=%d\n",_test_mavlink_rx.test2);
//   PX4_INFO("test3=%lf\n",(double)_test_mavlink_rx.test3);
    if(_test_mavlink_rx.test1==1&&_test_mavlink_rx.test2==2)
    {
        parameters_update();
        mavlink_log_info(&_mavlink_log_pub, "_param_mis_takeoff_alt.get():%f",(double)_param_mis_takeoff_alt.get());
        if(_param_mis_takeoff_alt.get()>5&&_param_mis_takeoff_alt.get()<30)
{
    fly_alt=_param_mis_takeoff_alt.get();
}
else
{
fly_alt=5;
}
_vehicle_status_sub.copy(&_status);
        if(_status.arming_state==vehicle_status_s::ARMING_STATE_ARMED)
        {
fly_alt=0;
        }
     //   PX4_INFO("fly_alt=%lf",(double)fly_alt);
        _vehicle_local_position_sub.copy(&_vehicle_local_position);
        _follow_target_sub.copy(&_target);
            //    _target.lat= 47.3978161;
            //    _target.lon=8.5460368;
        if(_vehicle_local_position.xy_valid)
{
        _global_local_proj_ref.initReference(_vehicle_local_position.ref_lat,_vehicle_local_position.ref_lon,hrt_absolute_time());
        _global_local_proj_ref.project(_target.lat,_target.lon,target_x,target_y);
               _a02_dist_to_target.dist_to_target=(double)sqrtf((_vehicle_local_position.x-target_x)*(_vehicle_local_position.x-target_x)+(_vehicle_local_position.y-target_y)*(_vehicle_local_position.y-target_y));
_a02_dist_to_target_pub.publish(_a02_dist_to_target);
            //  mavlink_log_info(&_mavlink_log_pub, "与目标的距离:%f",_a02_dist_to_target.dist_to_target);

        if(_a02_dist_to_target.dist_to_target<2000)
        {
          mavlink_log_info(&_mavlink_log_pub, "与目标的距离:%f",_a02_dist_to_target.dist_to_target);
                  break;
_a03_buoy_sate.state=5;
_a03_buoy_sate_pub.publish(_a03_buoy_sate);

        }
        else
        {
            mavlink_log_info(&_mavlink_log_pub, "与目标的距离:%f",_a02_dist_to_target.dist_to_target);
            mavlink_log_info(&_mavlink_log_pub, "与目标的距离太远");
        }

}
else
{
mavlink_log_info(&_mavlink_log_pub, "飞机未定位");
}

    }
  usleep(500000);

    }

    while(!should_exit())
    {
        _vehicle_status_sub.copy(&_status);
        if((_status.nav_state==vehicle_status_s::NAVIGATION_STATE_OFFBOARD)&&(_status.arming_state==vehicle_status_s::ARMING_STATE_ARMED))
        {
            mavlink_log_info(&_mavlink_log_pub, "已解锁");
            break;
        }
        _command.target_system = _status.system_id;
        _command.target_component = _status.component_id;
        ocm.timestamp = hrt_absolute_time();
        _vehicle_local_position_sub.copy(&_vehicle_local_position);
        sp_local.x=_vehicle_local_position.x;
        sp_local.y=_vehicle_local_position.y;
        sp_local.z=_vehicle_local_position.z-fly_alt;
        sp_local.timestamp = hrt_absolute_time();
        _trajectory_setpoint_pub.publish(sp_local);
        ocm.position=true;
        ocm.timestamp = hrt_absolute_time();
        _offboard_control_mode_pub.publish(ocm);
        _command.command =vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
        _command.param1=1.0f;
        _command.param2=PX4_CUSTOM_MAIN_MODE_OFFBOARD;
        _command.timestamp = hrt_absolute_time();
        _vehicle_command_pub.publish(_command);
        usleep(10000);
        _command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
        _command.param1 = 1.0f;
        _command.timestamp = hrt_absolute_time();
        _vehicle_command_pub.publish(_command);
    }
//记录当前点的位置
    _vehicle_local_position_sub.copy(&_vehicle_local_position);
    begin_x=_vehicle_local_position.x;
    begin_y=_vehicle_local_position.y;
    begin_z=_vehicle_local_position.z;
    _global_local_proj_ref.initReference(_vehicle_local_position.ref_lat,_vehicle_local_position.ref_lon,hrt_absolute_time());
    time_tick=hrt_absolute_time();

vehicle_attitude_s att;
		_att_sub.update(&att);
const matrix::Eulerf euler = matrix::Quatf(att.q);
begin_yaw=euler.psi();
PX4_INFO("begin_yaw=%lf",(double)begin_yaw);

 vel_max=_param_mpc_xy_vel_max.get();
 PX4_INFO("vel_max=%lf",(double)vel_max);
    //业务逻辑
    while (!should_exit())
    {
        _vehicle_local_position_sub.copy(&_vehicle_local_position);
        _follow_target_sub.copy(&_target);
            //    _target.lat= 47.3978161;
            //    _target.lon=8.5460368;
        _global_local_proj_ref.project(_target.lat,_target.lon,target_x,target_y);
        _a02_dist_to_target.dist_to_target=(double)sqrtf((_vehicle_local_position.x-target_x)*(_vehicle_local_position.x-target_x)+(_vehicle_local_position.y-target_y)*(_vehicle_local_position.y-target_y));
_a02_dist_to_target_pub.publish(_a02_dist_to_target);
_distance_sensor_sub.copy(&_distance_sensor);
            //  mavlink_log_info(&_mavlink_log_pub, "与目标的距离:%f",_a02_dist_to_target.dist_to_target);

        if(flag==0)//升至5米
        {
            flag++;
            memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
            sp_local.x=begin_x;
            sp_local.y=begin_y;
            sp_local.z=begin_z-fly_alt;
            sp_local.yaw=begin_yaw;
                    ocm.position=true;
                    ocm.velocity=false;
                    ocm.acceleration=false;
                   mavlink_log_info(&_mavlink_log_pub, "起飞");

        }
        if((flag==1)&&(abs(_vehicle_local_position.x-sp_local.x)<xy_rad)&&(abs(_vehicle_local_position.y-sp_local.y)<xy_rad)&&(abs(_vehicle_local_position.z-sp_local.z)<z_rad))//升至5米
        {
            if(flag2==0)
            {
                flag2++;
                time_tick=hrt_absolute_time();
                time_tick1=hrt_absolute_time();
                mavlink_log_info(&_mavlink_log_pub, "飞向目标");
            }
            flag++;
        }
        if((flag2==1))
        {


            memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
            if((gain>-0.0000001f)&&(gain<1.00000001f))
            {
            gain= (float)(hrt_absolute_time()-time_tick1)/5000000;
            }
            else
            {
               gain=1.00000001f;
            }

 cur_vel_max=vel_max*gain;
 _param_mpc_xy_vel_max.set(cur_vel_max);
  parameters_update();
 cur_vel_max=_param_mpc_xy_vel_max.get();
//   PX4_INFO("cur_vel_max=%lf",(double)cur_vel_max);
            // PX4_INFO("gain=%lf",(double)gain);

            // if((hrt_absolute_time()-time_tick)>5000000)
            // {

            // }
            // if(gain<1)
            if(gain>1)
            {
            sp_local.x=target_x;
            sp_local.y=target_y;
            }
            else
            {
            sp_local.x=begin_x+(target_x-begin_x)*gain;
            sp_local.y=begin_y+(target_y-begin_y)*gain;
            // PX4_INFO("(int)(gain*10)=%d",(int)(gain*10));
            // PX4_INFO("gainbuff[(int)(gain*10)]=%lf",(double)gainbuf[(int)(gain*10)]);

            }


            // if(gain>1)
            // {
            // sp_local.x=target_x;
            // sp_local.y=target_y;
            // }
            // PX4_INFO("target_x=%lf",(double)target_x);
            // PX4_INFO("target_y=%lf",(double)target_y);
            // PX4_INFO("sp_local.x=%lf",(double)sp_local.x);
            // PX4_INFO("sp_local.y=%lf",(double)sp_local.y);

            sp_local.z=begin_z-fly_alt;
            sp_local.yaw=begin_yaw;
            sp_local.vx=(float)NAN;
            sp_local.vy=(float)NAN;
            sp_local.vz=(float)NAN;
            sp_local.acceleration[0]=(float)NAN;
            sp_local.acceleration[1]=(float)NAN;
            sp_local.acceleration[2]=(float)NAN;
            ocm.position=true;
            ocm.velocity=false;
            ocm.acceleration=false;
            time_tick=hrt_absolute_time();

        }
        if((flag==2)&&(abs(_vehicle_local_position.x-target_x)<xy_rad)&&(abs(_vehicle_local_position.y-target_y)<xy_rad)&&(abs(_vehicle_local_position.z-sp_local.z)<z_rad))//升至5米
        {
            if(flag2==1)
            {
                flag2++;
                time_tick=hrt_absolute_time();
               mavlink_log_info(&_mavlink_log_pub, "降落到目标");

        }
        }
        if((flag2==2))
        {
            flag++;
            memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s));
            sp_local.x=target_x;
            sp_local.y=target_y;
            sp_local.z=NAN;
            sp_local.yaw=begin_yaw;
            sp_local.vx=(float)NAN;
            sp_local.vy=(float)NAN;
            sp_local.vz=(float)0.5;
            if(_distance_sensor.current_distance>0.0f&&_distance_sensor.current_distance<2.0f)
            {
                sp_local.vz=(float)0.3;
            }
            sp_local.acceleration[0]=(float)NAN;
            sp_local.acceleration[1]=(float)NAN;
            sp_local.acceleration[2]=(float)NAN;
                    ocm.position=true;
                    ocm.velocity=true;
                    ocm.acceleration=false;
            time_tick=hrt_absolute_time();

        }


if(ocm.position||ocm.velocity||ocm.acceleration)
{
        ocm.timestamp = hrt_absolute_time();
        _offboard_control_mode_pub.publish(ocm);
        _vehicle_status_sub.copy(&_status);
if(_status.nav_state==vehicle_status_s::NAVIGATION_STATE_OFFBOARD)
{
        sp_local.timestamp = hrt_absolute_time();
        _trajectory_setpoint_pub.publish(sp_local);
//        PX4_INFO("sp_local.x=%lf\n",(double)sp_local.x);
//        PX4_INFO("sp_local.y=%lf\n",(double)sp_local.y);
//        PX4_INFO("sp_local.z=%lf\n",(double)sp_local.z);
//        PX4_INFO("sp_local.vx=%lf\n",(double)sp_local.vx);
//        PX4_INFO("sp_local.vy=%lf\n",(double)sp_local.vy);
//        PX4_INFO("sp_local.vz=%lf\n",(double)sp_local.vz);
}
}
        usleep(50000);

        parameters_update();
    }
}

void ControlNode::parameters_update(bool force)
{
    // check for parameter updates
    if (_parameter_update_sub.updated() || force)
    {
        // clear update
        parameter_update_s update;
        _parameter_update_sub.copy(&update);

        // update parameters from storage
        updateParams();
    }
}

int ControlNode::print_usage(const char *reason)
{
    if (reason)
    {
        PX4_WARN("%s\n", reason);
    }

    PRINT_MODULE_DESCRIPTION(
        R"DESCR_STR(
### Description
Section that describes the provided module functionality.

This is a template for a module running as a task in the background with start/stop/status functionality.

### Implementation
Section describing the high-level implementation of this module.

### Examples
CLI usage example:
$ module start -f -p 42

)DESCR_STR");

    PRINT_MODULE_USAGE_NAME("module", "template");
    PRINT_MODULE_USAGE_COMMAND("start");
    PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
    PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);
    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

    return 0;
}

int control_node_main(int argc, char *argv[])
{
    return ControlNode::main(argc, argv);
}


